package main

import (
	"container/list"
	"fmt"
	"math"
)

const ER = 6378.137  //地球平均半径
const PI = math.Pi

type testCase struct {
	A_j float64  //A经度
	A_w float64  //A纬度
	B_j float64  //B经度
	B_w float64  //B纬度
	L float64  //A,B距离
	bearing float64  //方位角
}

//角度转弧度
func rad(d float64) float64{
	return d * PI / 180.0
}

//弧度转角度
func deg(x float64)float64{
	return x * 180 / PI
}

//求余操作
func MOD(n float64, d int)(float64){
	t := (n / float64(d))
	x := n - float64(d * int(t))
	//fmt.Printf("%v mod %v = %v \n",n,d,x)
	return x
}

//经纬度转弧度
func jw2Rad(lng1, lat1, lng2, lat2 float64)(float64,float64,float64,float64){
	lng1 = rad(lng1)
	lat1 = rad(lat1)
	lng2 = rad(lng2)
	lat2 = rad(lat2)
	return lng1, lat1, lng2, lat2
}

//Q1: 已知A,B两点经纬度，求AB距离
func cal_L(lng1, lat1, lng2, lat2 float64)(float64){
	lng1, lat1, lng2, lat2 = jw2Rad(lng1, lat1, lng2, lat2)
	dlon := lng2 - lng1
	dlat := lat2 - lat1
	a := math.Pow(math.Sin(dlat / 2),2) + math.Cos(lat1) * math.Cos(lat2) * math.Pow(math.Sin(dlon / 2),2)
	distance := 2 * math.Asin(math.Sqrt(a)) * ER * 1000
	//distance = math.Round(distance)
	return distance
}

//Q2: 已知A,B两点经纬度，求B点相对于A的航向角
func cal_Bearing(lonA, latA, lonB, latB float64)(float64){
	radLatA := rad(latA)
	radLonA := rad(lonA)
	radLatB := rad(latB)
	radLonB := rad(lonB)
	dLon := radLonB - radLonA
	y := math.Sin(dLon) * math.Cos(radLatB)
	x := math.Cos(radLatA) * math.Sin(radLatB) - math.Sin(radLatA) * math.Cos(radLatB) * math.Cos(dLon)
	brng := deg(math.Atan2(y, x))
	brng = MOD((brng + 360),360)
	return brng
}

//已知A点经纬度，和A点到B点的距离和航向角，求B点经纬度
func cal_B_jw(lat, lng, brng, dist float64)(float64,float64){
	a := ER * 1000
	b := 6356752.3142
	f := 1.0 / 298.257223563

	lon1 := lng * 1
	lat1 := lat * 1
	s := dist
	alpha1 := rad(brng)
	sinAlpha1 := math.Sin(alpha1)
	cosAlpha1 := math.Cos(alpha1)

	tanU1 := (1 - f) * math.Tan(rad(lat1))
	cosU1 := 1 / math.Sqrt((1 + tanU1 * tanU1))
	sinU1 := tanU1 * cosU1
	sigma1 := math.Atan2(tanU1, cosAlpha1)
	sinAlpha := cosU1 * sinAlpha1
	cosSqAlpha := 1 - sinAlpha * sinAlpha
	uSq := cosSqAlpha * (a * a - b * b) / (b * b)
	A := 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
	B := uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))

	sigma := s / (b * A)
	sigmaP := 2 * PI
	sinSigma, cos2SigmaM,cosSigma,deltaSigma := 0.0,0.0,0.0,0.0
	for math.Abs(sigma - sigmaP) > 1e-12{
		cos2SigmaM = math.Cos(2 * sigma1 + sigma)
		sinSigma = math.Sin(sigma)
		cosSigma = math.Cos(sigma)
		deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
			B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (
				-3 + 4 * cos2SigmaM * cos2SigmaM)))
		sigmaP = sigma
		sigma = s / (b * A) + deltaSigma
	}

	tmp := sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1
	lat2 := math.Atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
		(1 - f) * math.Sqrt(sinAlpha * sinAlpha + tmp * tmp))
	lambdA := math.Atan2(sinSigma * sinAlpha1, cosU1*cosSigma - sinU1 * sinSigma * cosAlpha1)
	C := f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
	L := lambdA - (1-C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM+C * cosSigma * (-1+2 * cos2SigmaM * cos2SigmaM)))
	//revAz := math.Atan2(sinAlpha, -tmp)

	lat,lon := lon1 + deg(L), 180 + deg(lat2)
	return lon,lat
}

func main(){
	//A_w,A_j,B_w,B_j := 31.482526, 117.114355, 31.471631, 117.171348
	////经纬度报文转经纬度
	//L,bearing := 8935.29, 103.79  //单位m,rad

	testCases := list.New()

	t1 := testCase{A_j:117.114355,A_w:31.482526,B_j:117.171348,B_w:31.471631,L:8935.29,bearing:103.79}
	t2 := testCase{A_j:117.195422,A_w:31.807031,B_j:117.263710,B_w:31.809692,L:6472.98,bearing:87.35}
	t3 := testCase{A_j:117.114355,A_w:31.482526,B_j:117.122880,B_w:31.543221,L:11496.32,bearing:5.91}
	t4 := testCase{A_j:117.114355,A_w:31.482526,B_j:117.81083,B_w:31.543221,L:12610.30,bearing:333.80}
	t5 := testCase{A_j:119.5607,A_w:21.9311,B_j:112.5956,B_w:12.0297,L:12610.30,bearing:333.80}
	t6 := testCase{A_j:104.1937,A_w:20.8583,B_j:112.5956,B_w:12.0297,L:1000000,bearing:30}

	testCases.PushBack(t1)
	testCases.PushBack(t2)
	testCases.PushBack(t3)
	testCases.PushBack(t4)
	testCases.PushBack(t5)
	testCases.PushBack(t6)

	i := 0
	for e := testCases.Front(); e != nil; e = e.Next() {

		fmt.Printf("#################### testcase%v ##################\n",i)
		t := (e.Value).(testCase)
		A_w,A_j,B_w,B_j,L,bearing := t.A_w, t.A_j, t.B_w, t.B_j, t.L, t.bearing

		//已知A,B两点经纬度，求AB距离,航向角
		res_L := cal_L(A_j,A_w,B_j,B_w)
		res_bearing := cal_Bearing(A_j,A_w,B_j,B_w)
		fmt.Printf("A,B计算距离 = %v m, 实际距离 = %v m\n",res_L,L)
		fmt.Printf("A,B计算航向角 = %v°, 实际航向角 = %v° \n",res_bearing,bearing)

		//已知A点经纬度，和A点到B点的距离和航向角，求B点经纬度
		res_B_j, res_B_w := cal_B_jw(A_j,A_w,L,bearing)
		fmt.Printf("B点计算经度 = %v, 实际经度 = %v \n",res_B_j,B_j)
		fmt.Printf("B点计算纬度 = %v, 实际纬度 = %v \n\n",res_B_w,B_w)

		i += 1
	}
}